package com.example.llademo.util;

import static com.lzhy.coordinate.sdk.CoordinateSDK.BLH2RAE;

/**
 * 计算经纬度距离
 */
public class CalculateUtils {
    /**
     * 计算两个位置点之间的方位角
     * 大于56度则卫星信号覆盖不到
     *
     * @param lat1 第一个位置点的纬度
     * @param lng1 第一个位置点的经度
     * @param alt1 第一个位置点的高度
     * @param lat2 第二个位置点的纬度
     * @param lng2 第二个位置点的经度
     * @param alt2 第二个位置点的高度
     * @return 两个位置点之间的方位角（度数）
     */
    public static double angle(double lat1, double lng1, double alt1, double lat2, double lng2, double alt2) {
        // 将第一个和第二个位置点的经纬度和高度转换为球面坐标系中的BLH（海拔，纬度，经度）格式
        double[] BLH_flight = new double[]{lat1, lng1, alt1};
        double[] BLH_satellite = new double[]{lat2, lng2, alt2};
        // 将BLH格式转换为RAE（方位角，仰角，极化角）格式，用于计算方位角
        double[] rae = BLH2RAE(BLH_flight[0], BLH_flight[1], BLH_flight[2], BLH_satellite[0], BLH_satellite[1], BLH_satellite[2]);
        // 由于方位角的计算结果需要加上90度，将其转换为实际的方位角
        rae[2] = rae[2] + 90;
        return rae[2];
    }

    public static double Distance(double lat1, double lon1, double lat2, double lon2) {
        //用haversine公式计算球面两点间的距离。
        //经纬度转换成弧度
        lat1 = ConvertDegreesToRadians(lat1);
        lon1 = ConvertDegreesToRadians(lon1);
        lat2 = ConvertDegreesToRadians(lat2);
        lon2 = ConvertDegreesToRadians(lon2);
        //差值
        double vLon = Math.abs(lon1 - lon2);
        double vLat = Math.abs(lat1 - lat2);

        //h is the great circle distance in radians, great circle就是一个球体上的切面，它的圆心即是球心的一个周长最大的圆。
        double h = HaverSin(vLat) + Math.cos(lat1) * Math.cos(lat2) * HaverSin(vLon);

        return 2 * 6371.0 * Math.asin(Math.sqrt(h));
    }

    private static double HaverSin(double theta) {
        double v = Math.sin(theta / 2);
        return v * v;
    }

    private static double ConvertDegreesToRadians(double degrees) {
        return degrees * Math.PI / 180;
    }

    public static double GetAngleFromLatLngPoints(double lat0, double lng0, double lat1, double lng1) {
        double disxy = Distance(lat0, lng0, lat1, lng1);
        double disy = Distance(lat0, lng0, lat1, lng0);
        double angle = Math.acos(disy / disxy) * 180 / 3.141592653;
        double x1, x2, y1, y2;
        x1 = lng0;
        y1 = lat0;
        x2 = lng1;
        y2 = lat1;
        if (x1 < x2 && y1 < y2) {
        } else if (x1 < x2 && y1 > y2) {
            angle = 180 - angle;
        } else if (x1 > x2 && y1 > y2) {
            angle = 180 + angle;
        } else if (x1 > x2 && y1 < y2) {
            angle = 360 - angle;
        }
        return angle;
    }
}
